/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-06-00     YTR       the first version
 */

#include "main.h"
#include <all.h>
#include <rt_uart.h>

uint8_t old_mode = 0;
void Robot_stand(){
    uart6_tx((uint8_t *)"DGS:1!");
    rt_thread_mdelay(100);
}
void Robot_forword(int frequency){
    uint8_t buff[64];
    sprintf((char *)buff,"$DKT:1,%d!",frequency);
    uart6_tx(buff);
    rt_thread_mdelay(100);
}
void Robot_retreat(int frequency){
    uint8_t buff[64];
    sprintf((char *)buff,"$DKT:2,%d!",frequency);
    uart6_tx(buff);
    rt_thread_mdelay(100);
}
void Robot_turn_left(int frequency){
    uint8_t buff[64];
    sprintf((char *)buff,"$DKT:3,%d!",frequency);
    uart6_tx(buff);
    rt_thread_mdelay(100);
}
void Robot_turn_right(int frequency){
    uint8_t buff[64];
    sprintf((char *)buff,"$DKT:4,%d!",frequency);
    uart6_tx(buff);
    rt_thread_mdelay(100);
}
void Robot_Mode(uint8_t mode){
    if(mode != old_mode){
        Robot_stand();
    }
    switch(mode){
    case 0:
        Robot_stand();
        break;
    case 1:
        Robot_forword(1);
        break;
    case 2:
        Robot_retreat(1);
        break;
    case 3:
        Robot_turn_left(1);
        break;
    case 4:
        Robot_turn_right(1);
        break;
    }
    old_mode = mode;
}
